/*
 * Copyright 2020 Makani Technologies LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CONTROL_ACTUATOR_UTIL_H_
#define CONTROL_ACTUATOR_UTIL_H_

#include <stdbool.h>

#include "common/c_math/vec3.h"
#include "control/actuator_types.h"
#include "control/system_types.h"
#include "system/labels.h"

#ifdef __cplusplus
extern "C" {
#endif

// Applies some basic sanity checks to the RotorControlParams values.
//
// Args:
//   params: Parameter structure to test.
//
// Returns:
//   True if the parameters satisfy the basic checks.  This function
//   will try to assert out any of these tests fail.
bool ActuatorUtilValidateParams(const RotorControlParams *params);

// Computes the expected maximum thrust for a given apparent wind.
double CalcMaxTotalThrust(double v_app, const RotorControlParams *params);

// Converts a thrust-moment command and a set of weights to a set of
// rotor speeds.
//
// Args:
//   thrust_moment: Thrust [N] and moment [N-m] command.
//   weights: Weight to place on each component ([1/N^2] for thrust and
//       [1/(N^2-m^2)] for moment).
//   v_app: Magnitude of the apparent wind [m/s].  The hover controller
//       should set this to 0.0 to avoid using extra sensors.
//   pqr: Angular rate [rad/s] vector.
//   stacking_state: Enum describing which, if any, stacked motor
//       blocks are faulted.
//   force_zero_advance_ratio: Indicates that zero apparent wind
//       should be assumed for each motor and complete motor shut-off
//       is allowed.
//   air_density: Air density [kg/m^3].
//   rotor_params: Array of physical parameters for each rotor, which
//       includes their position and the expect C_P variation expected
//       at their position.  This is used to calculate the local
//       inflow velocity at the rotor.
//   params: Parameters used for rotor control.
//   rotors: Rotor angular rates [rad/s].
//   available_thrust_moment: Output total thrust and moments
//       generated by thrusts.
void MixRotors(const ThrustMoment *thrust_moment, const ThrustMoment *weights,
               double v_app, const Vec3 *pqr, StackingState stacking_state,
               bool force_zero_advance_ratio, double air_density,
               const RotorParams *const rotor_params[],
               const RotorControlParams *params, double *rotors,
               ThrustMoment *available_thrust_moment, double *v_app_locals);

// Converts the logical flap grouping deltas (e.g. delta_ail,
// delta_ele) to physical flap deflections and applies a saturation to
// their deflections.
//
// Args:
//   deltas: Angles that the flaps are deflected.  For the ailerons,
//       positive means that the starboard aileron is down and the
//       port aileron is up.  For the other deltas, a positive sign
//       corresponds to a positive rotation about the relevant body
//       axis.
//   offsets: Flap offsets for zero deltas.
//   lower_limits: Lower saturation limits.
//   upper_limits: Upper saturation limit.
//   flaps: Output variable to return the flaps.
//   deltas_available: Achieved deltas [structure of rad].
void MixFlaps(const Deltas *deltas, const double *offsets,
              const double *lower_limits, const double *upper_limits,
              double *flaps, Deltas *deltas_available);

// Converts servo angles to corresponding flap angles.
//
// There is not a 1:1 mapping between servos and flaps, as the
// rudder and elevator are each driven by two servos.  To estimate
// the rudder and elevator positions, average the positions of their
// respective servo pairs.
//
// The rudder servo angle to rudder angle is nonlinear due to a 4-bar linkage.
// All other control surfaces have a linear relation with servo angle.
//
// Args:
//   servo_angles[kNumServos]: Servo deflection angles.
//   params[kNumServos]: Parameters for each servo.
//   flap_angles[kNumFlaps]: Output flap deflection angles.
void ServoAnglesToFlapAngles(const double servo_angles[],
                             const ServoParams params[], double flap_angles[]);

// Converts flap angles to servo angles.
//
// This function is the inverse of the servo to flap function above, modulo
// the paired servos which are each calculated directly.
//
// Args:
//   flap_angles[kNumFlaps]: Flap deflection angles.
//   params[kNumServos]: Parameters for each servo.
//   servo_angles[kNumServos]: Output servo deflection angles.
void FlapAnglesToServoAngles(const double flap_angles[],
                             const ServoParams params[], double servo_angles[]);

// Returns the target azimuth for GS02 when the kite is in hover.
//
// Args:
//   wing_pos_g: The position of the wing in the g-frame.
//   params: Paramters for GS02.
//   target_valid: Whether the result is valid.
double CalcHoverGsTargetAzimuthReel(const Vec3 *vessel_pos_g,
                                    const Mat3 *dcm_g2v, const Vec3 *wing_pos_g,
                                    const Gs02Params *params,
                                    bool *target_valid);

#ifdef __cplusplus
}  // extern "C"
#endif

#endif  // CONTROL_ACTUATOR_UTIL_H_
